﻿using System;
using System.Collections.Generic;
using System.Text;
using Geometry;
using FIRADriverLibrary;


namespace AGHFiraPlayer.Strategies
{
    class SimpleFunctions
    {
        public static double MAGIC_NUMBER = 200;
        public static double TOO_LONG = 2;
        private static double ROBOT_DIAMETER = 0.075*3;
        private static Point2D MIDDLE_OF_GOAL = new Point2D(Predicator.pitchArea.Right+0.2, Predicator.pitchArea.Height / 2.0);
        private static Point2D GOAL_LEFT = new Point2D(Predicator.pitchArea.Right, Predicator.pitchArea.Height / 2.0 - 0.5);
        private static Point2D GOAL_RIGHT = new Point2D(Predicator.pitchArea.Right, Predicator.pitchArea.Height / 2.0 + 0.5);
        private static AGHDriver driver;
        private static bool initCalled = false;
        public static void init(AGHDriver driver)
        {
            SimpleFunctions.driver = driver;
            SimpleFunctions.initCalled = true;
        }
        //return double if bigger the better position
        public static double judgePosition(RobotController robot)
        {
            double COS_FACTOR = 5;

            if (!initCalled)
            {
                throw new Exception("Init method has to be called before using any SimpleFunctions class methods");
            }

            Vector2D robotVector = new Vector2D(robot.robotState.angle);
            double robotSpeed = (robot.robotState.currentLeftSpeed + robot.robotState.currentRightSpeed) / 2.0;
            if (robotSpeed < 0)
            {
                robotVector.Rotate(Math.PI / 2);
            }
            Point2D robotPosition = new Point2D(robot.robotState.x, robot.robotState.y);
            Vector2D toGoalVector = new Vector2D(robotPosition, MIDDLE_OF_GOAL);
            //[0,2]
            double angelFactor = (Math.Cos(robotVector.AngleBetween(toGoalVector))) * COS_FACTOR + COS_FACTOR;

            double oponents = 1;

            robotVector.Rotate(Math.PI / 2);

            Point2D a1 = new Point2D(robotPosition.x + robotVector.x * ROBOT_DIAMETER, robotPosition.y + robotVector.y * ROBOT_DIAMETER);
            Point2D a2 = new Point2D(robotPosition.x - robotVector.x * ROBOT_DIAMETER, robotPosition.y - robotVector.y * ROBOT_DIAMETER);
            Polygon2D polygon = new Polygon2D(new Point2D[] { robotPosition, GOAL_LEFT, GOAL_RIGHT });
            foreach (RobotController oponent in driver.Oponents)
            {
                Point2D oponentPosition = new Point2D(oponent.robotState.x, oponent.robotState.y);
                if (polygon.Contains(oponentPosition))
                {
                    oponents += 1;
                }
            }
            //0-400
            double result = (100 * toGoalVector.Length * oponents * COS_FACTOR / (angelFactor + 1.01));
            if (result > 2 * MAGIC_NUMBER)
            {
                result = 2 * MAGIC_NUMBER;
            }
            //[-200.200]
            return (-1.0 * result) + MAGIC_NUMBER;
        }

        public static bool isAnybodyOnTheWay(RobotController robot, Point2D destiny)
        {
            Point2D robotPosition = new Point2D(robot.robotState.x, robot.robotState.y);
            Vector2D vec = (new Vector2D(robot.robotState.angle));
            vec.Rotate(Math.PI / 2);

            Point2D a1 = new Point2D(robotPosition.x + vec.x * ROBOT_DIAMETER,robotPosition.y + vec.y * ROBOT_DIAMETER);
            Point2D a2 = new Point2D(robotPosition.x - vec.x * ROBOT_DIAMETER, robotPosition.y - vec.y * ROBOT_DIAMETER);

            Point2D a3 = new Point2D(destiny.x + vec.x * ROBOT_DIAMETER, destiny.y + vec.y * ROBOT_DIAMETER);
            Point2D a4 = new Point2D(destiny.x - vec.x * ROBOT_DIAMETER, destiny.y - vec.y * ROBOT_DIAMETER);

            Polygon2D polygon = new Polygon2D(new Point2D[] { a1,a2,a3,a4 });
            foreach (RobotController oponent in driver.Oponents) {
                Point2D oponentPosition = new Point2D(oponent.robotState.x, oponent.robotState.y);
                if (polygon.Contains(oponentPosition))
                {
                    return true;
                }
            }
            foreach (RobotController rob in driver.Robots) {
                if (rob.Equals(robot)) {
                    continue;
                }
                Point2D robPosition = new Point2D(rob.robotState.x, rob.robotState.y);
                if (polygon.Contains(robPosition))
                {
                    return true;
                }
            }
            return false;
        }

        public static GeometryCSharp.Point2D calculateShotPoint(RobotController robot, GeometryCSharp.Point2D destiny, Double eta)
        {
            GeometryCSharp.Point2D ballPosition;
            GeometryCSharp.Point2D shotPoint = null;
            GeometryCSharp.Vector2D toGoal;
            while (shotPoint == null)
            {
                eta += 0.01;
                if (eta > TOO_LONG)
                {
                    break;
                }
                ballPosition = Predicator.GetBallPosition(eta);
                toGoal = new GeometryCSharp.Vector2D(ballPosition, destiny);
                toGoal.Normalize();
                GeometryCSharp.BezierCurve bc = BezierCurveDriver.calculateBezzierCurve(robot.robotState, ballPosition, PotencialDriver.maxVelocity, toGoal);
                double distance;
                if (bc == null)
                {
                    continue;
                }
                else
                {
                    distance = bc.calculateLength(1000);
                    double calculatedEta = distance / PotencialDriver.maxVelocity;
                    if (calculatedEta < eta)
                    {
                        shotPoint = ballPosition;
                    }
                }

            }
            return shotPoint;
        }
    }
}
